//
// Created by whitby on 7/25/23.
//

#ifndef DATA_PROCESS_UTILS_H
#define DATA_PROCESS_UTILS_H

#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>

using namespace std;

struct IMU {
    IMU() = default;

    double timestamp{};
    float acc[3]{};
    float gyro[3]{};
};

struct Gravity {
    Gravity() = default;

    double timestamp{};
    float data[3]{};
};

inline std::istream &operator>>(std::istream &input, IMU &imu) {
    input >> imu.timestamp >> imu.gyro[0] >> imu.gyro[1] >> imu.gyro[2] >> imu.acc[0] >> imu.acc[1] >> imu.acc[2];
    return input;
}

inline std::ostream &operator<<(std::ostream &output, const IMU &imu) {
    output << imu.timestamp << " " << imu.gyro[0] << " " << imu.gyro[1] << " " << imu.gyro[2] << " " << imu.acc[0]
           << " " << imu.acc[1] << " " << imu.acc[2];
    return output;
}

inline std::istream &operator>>(std::istream &input, Gravity &gravity) {
    input >> gravity.timestamp >> gravity.data[0] >> gravity.data[1] >> gravity.data[2];
    return input;
}

inline std::ostream &operator<<(std::ostream &output, const Gravity &gravity) {
    output << gravity.timestamp << " " << gravity.data[0] << " " << gravity.data[1] << " " << gravity.data[2];
    return output;
}

void loadTimeFile(const std::string &timeFilePath, std::vector<double> &timeVec) {
    ifstream fin(timeFilePath);
    while (!fin.eof()) {
        string lineStr;
        getline(fin, lineStr);
        if (!lineStr.empty()) {
            stringstream ss(lineStr);
            double timestamp;
            ss >> timestamp;
            timeVec.push_back(timestamp);
        }
    }
    fin.close();
}

void loadIMUFile(const std::string &imuFilePath, std::vector<IMU> &imuVec) {
    ifstream fin(imuFilePath);
    double last_t = 0;
    while (!fin.eof()) {
        string lineStr;
        getline(fin, lineStr);
        if (!lineStr.empty()) {
            stringstream ss(lineStr);
            IMU imu;
            ss >> imu;
            if (imu.timestamp > last_t) {
                last_t = imu.timestamp;
                imuVec.emplace_back(imu);
            }
        }
    }
    fin.close();
}

void loadGravityFile(const std::string &gravityFilePath, std::vector<Gravity> &gravityVec) {
    ifstream fin(gravityFilePath);
    double last_t = 0;
    while (!fin.eof()) {
        string lineStr;
        getline(fin, lineStr);
        if (!lineStr.empty()) {
            stringstream ss(lineStr);
            Gravity gravity;
            ss >> gravity;
            if (gravity.timestamp > last_t) {
                last_t = gravity.timestamp;
                gravityVec.emplace_back(gravity);
            }
        }
    }
    fin.close();
}

#endif //DATA_PROCESS_UTILS_H
